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This  paper  is  concerned  with  tracking  of  ground  targets  on  roads 
and  investigates  possible  ways  to  improve  target  state  estimation  via 
fusing  a  target’s  track  with  information  about  the  road  along  which 
the  target  is  traveling.  A  target  track  is  estimated  using  a  surveil¬ 
lance  radar  whereas  a  digital  map  provides  the  road  network  of  the 
region  under  surveillance.  When  the  information  about  roads  is  as 
accurate  as  (or  even  better  than)  radar  measurements,  it  is  desired 
naturally  to  incorporate  such  information  (fusion)  into  target  state 
estimation.  In  this  paper,  roads  are  modeled  with  analytic  functions 
and  their  fusion  with  a  target  track  is  cast  as  linear  or  nonlinear 
state  constraints  in  an  optimization  procedure.  The  constrained  op¬ 
timization  is  then  solved  with  the  Lagrangian  multiplier,  leading  to 
a  closed-form  solution  for  linear  constraints  and  an  iterative  solu¬ 
tion  for  second-order  nonlinear  constraints.  Geometric  interpreta¬ 
tions  of  the  solutions  are  provided  for  special  cases.  Compared  to 
other  methods,  the  track-to-road  fusion  using  the  constrained  opti¬ 
mization  technique  can  be  easily  implemented  as  an  add-on  mod¬ 
ule  without  changes  to  an  existing  tracker.  For  curved  roads  with 
coarse  waypoints,  the  nonlinear  constrained  solution  outperforms 
the  piecewise  linearized  constrained  approach.  Computer  simula¬ 
tion  results  are  presented  to  illustrate  the  algorithms. 
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With  the  rapid  building  up  of  geographic  informa¬ 
tion  system  (GIS)  including  digital  road  maps  (DRM) 
and  digital  terrain  elevation  data  (DTED),  information 
about  roads  becomes  more  accurate,  up  to  date,  and 
accessible.  Looking  for  a  map  in  the  Internet  is  at  fin¬ 
gertips  with  a  least  cost  (i.e.,  distance  or  time)  route 
plotted  to  a  destination.  Road  and  terrain  information 
has  been  used  in  the  past  for  navigation  via  terrain  con¬ 
tour  matching.  Other  examples  include  the  increasingly 
popular  use  of  digital  maps  for  automobile  navigation 
with  a  Global  Positioning  System  (GPS)  receiver  and 
terrain-aided  navigation  for  aircraft. 

This  paper  is  concerned  with  tracking  of  ground  tar¬ 
gets  on  roads  and  investigates  possible  ways  to  improve 
target  state  estimation  via  fusing  a  target’s  track  with  in¬ 
formation  about  the  road  along  which  the  target  is  trav¬ 
eling.  A  target  track  is  estimated  using  a  surveillance 
radar  whereas  a  digital  map  provides  the  road  network 
of  the  region  under  surveillance.  Target  tracking  is  not 
unfamiliar  with  road  maps.  For  example,  target  tracks 
are  represented  by  colorful  dots  and  lines  blinking  along 
road  networks  on  a  big  screen,  often  on  top  of  a  topo¬ 
graphic  or  satellite  image,  in  a  situation  room,  in  an  air 
traffic  control  tower,  and  on  a  radar  operator  screen. 
In  these  applications,  however,  target  tracks  and  road 
networks  are  merely  displayed  together  with  little  or  no 
interaction  in  the  data  processing  level. 

When  the  information  about  roads  is  as  accurate  as 
(or  even  better  than)  radar  measurements,  it  is  naturally 
desired  to  incorporate  such  information  into  target  state 
estimation.  When  a  vehicle  travels  off-road  or  on  an 
unknown  road,  the  state  estimation  problem  is  uncon¬ 
strained.  However,  when  the  vehicle  is  traveling  on  a 
known  road,  be  it  straight  or  curved,  the  state  estima¬ 
tion  problem  can  be  cast  as  constrained  with  the  road 
network  information  available  from  digital  road/terrain 
maps.  In  the  past,  such  constraints  are  often  ignored  (or 
left  for  the  users  to  perceive  it  as  in  the  display  exam¬ 
ple  mentioned  above).  The  resulting  estimates,  even  ob¬ 
tained  with  the  Kalman  filter,  cannot  be  optimal  because 
they  do  not  make  full  use  of  this  additional  information 
about  state  constraints. 

To  use  such  state  constraints,  previous  attempts  can 
be  put  into  several  groups.  The  first  group  is  to  incorpo¬ 
rate  road  information  into  the  state  estimation  process. 
One  technique  is  to  reduce  the  system  model  param¬ 
eterization.  Another  technique  is  to  translate  the  state 
constraints  onto  the  state  process  and/or  observation 
noise  covariance  matrix  for  the  estimation  filter  [10]. 
The  use  of  variable  structure  IMM  (VS-IMM)  methods 
also  belongs  to  this  group  [7,  18,  19,  22].  Yet  another 
technique  is  to  project  a  dynamic  system  onto  linear 
state  constraints  and  then  apply  the  Kalman  filter  to 
the  projected  systems  [11].  Similarly,  for  nonlinear  state 
constraints,  there  is  the  one-dimensional  (ID)  represen¬ 
tation  of  a  target  motion  along  a  curvilinear  road  [27]. 
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The  technique  to  model  bounded  random  variables  with 
truncated  densities  also  belongs  to  this  group,  which  is 
easily  workable  with  such  nonlinear  filters  as  a  parti¬ 
cle  filter  [2,  4,  25,  26].  Road  map  information  can  also 
be  integrated  within  Multi-Hypothesis  Tracking  (MHT) 
[12,  13]. 

The  second  group  is  to  treat  state  constraints  as 
pseudo  measurements  [8].  For  a  road  segment,  its  an¬ 
alytic  model  not  only  constrains  the  target  position  but 
also  the  direction  of  the  target’s  velocity.  Indeed,  the 
target  velocity  is  closely  aligned  with  the  road  orien¬ 
tation  for  a  linear  segment  and  with  the  tangent  vec¬ 
tor  at  the  target  position  for  a  nonlinear  segment.  Fur¬ 
thermore,  an  estimate  of  centripetal  acceleration  can 
be  obtained  given  the  curvature  and  the  target  speed 
[27]. 

In  the  third  group,  an  unconstrained  Kalman  filter 
solution  is  first  obtained  and  then  the  unconstrained 
state  estimate  is  projected  onto  the  constrained  sur¬ 
face  [24].  This  technique  can  also  be  viewed  as  post¬ 
processing  (estimation  or  updating)  correction  [28]  or 
track-to-road  fusion  as  referred  to  in  this  paper.  In  con¬ 
ventional  track  fusion,  two  or  more  tracks  are  available, 
each  consisting  of  an  estimate  of  the  underlying  tar¬ 
get  trajectory  with  its  estimation  error  covariance.  The 
fused  track  is  typically  found  that  minimizes  the  sum 
of  CO  variance- weighted  state  errors  squared  [5,  6].  In 
contrast  to  this  conventional  track  fusion  that  operates 
on  individual  states,  fusion  of  tracks  with  roads  involves 
a  state  value  (a  point)  and  a  subset  of  state  values  (an 
arc  or  interval).  In  this  paper,  roads  are  modeled  with 
analytic  functions  and  their  fusion  with  a  target  track 
is  therefore  formulated  as  linear  or  nonlinear  state  con¬ 
straints  in  an  optimization  procedure. 

Although  this  paper  presents  a  new  technique  for 
the  third  group,  it  is  interesting  to  think  of  it  relative 
to  the  first  group  in  much  the  same  way  track  fusion 
is  compared  with  measurement  fusion.  In  measurement 
fusion,  measurements  from  all  sensors  are  made  avail¬ 
able  to  a  centralized  tracker,  which  has  the  potential  to 
fuse  out  the  best  estimate.  However,  measurement  fu¬ 
sion  may  not  be  practical  for  distributed  sensors  wherein 
gathering  all  raw  measurements  is  often  limited  by  net¬ 
work  transmission  bandwidth  and  latency.  Track  fusion 
is  frequently  used  as  acceptable  compromise  between 
performance  and  cost. 

Similarly,  fusion  of  tracks  with  road  constraints  (in 
the  third  group)  may  not  perform  as  well  as  an  algorithm 
that  incorporates  road  maps  directly  into  the  filtering 
process  (in  the  first  group).  However,  it  has  many  merits 
of  its  own.  First,  it  is  simple  and  can  be  retrofitted 
into  existing  trackers  as  an  add-on  module  without 
changes  to  the  trackers.  Since  the  tracks  are  obtained 
without  constraints,  it  can  easily  switch  between  off¬ 
road  and  on-road  operations  when  road  information  is 
available  and  the  unconstrained  tracks  are  deemed  close 
to  roads.  Second,  an  up-to-date  accurate  road  map  may 
not  be  available  to  individual  sensors  but  only  at  a 
fusion  center.  In  this  case,  the  algorithm  of  track-to- 


road  fusion  as  presented  in  this  paper  can  be  applied 
directly  whereas  those  in  the  first  group  cannot.  Third, 
as  noted  in  [18],  the  IMM  methods  based  on  road  maps 
do  not  always  perform  better  than  those  without  road 
maps  particularly  when  the  updating  interval  is  long.  In 
contrast,  constraining  an  on-road  target’s  track  onto  a 
road  (fusing)  has  no  such  a  problem.  Fourth,  the  track- 
to-road  fusion  algorithm  goes  beyond  target  tracking  to 
navigation  for  instance  where  it  can  be  used  to  loosely 
integrate  GPS  fixes  and  digital  maps  [16]. 

In  this  paper,  we  therefore  focus  on  the  third  group 
and  in  particular  present  an  optimization  procedure  for 
nonlinear  state  constraints  which  is  shown  to  be  superior 
to  the  linear  approximation  of  nonlinear  state  constraints 
as  suggested  in  [24]. 

There  are  a  host  of  constrained  nonlinear  optimiza¬ 
tion  techniques  [15].  Primal  methods  search  through  the 
feasible  region  determined  by  the  constraints.  Penalty 
and  barrier  methods  approximate  constrained  optimiza¬ 
tion  problems  by  unconstrained  problems  through  mod¬ 
ifying  the  objective  function  (e.g.,  add  a  term  for  higher 
price  if  a  constraint  is  violated).  Instead  of  the  origi¬ 
nal  constrained  problem,  dual  methods  attempt  to  solve 
an  alternate  problem  (the  dual  problem)  whose  un¬ 
knowns  are  the  Lagrangian  multipliers  of  the  first  prob¬ 
lem.  Cutting  plane  algorithms  work  on  a  series  of  ever- 
improving  approximating  linear  programs  whose  so¬ 
lutions  converge  to  that  of  the  original  problem.  La¬ 
grangian  relaxation  methods  are  widely  used  in  discrete 
constrained  optimization  problems. 

In  addition,  moving  horizon  estimation  reformulates 
the  estimation  problem  as  quadratic  programming  over 
a  moving,  fixed- size  estimation  window  and  has  be¬ 
come  an  important  approach  to  constrained  nonlinear 
estimation  [20].  Another  approach  to  constrained  linear 
estimation  is  to  exploit  the  Lagrangian  duality.  Indeed, 
a  constrained  linear  estimation  problem  is  shown  to  be 
a  particular  nonlinear  optimal  control  problem  in  [9]. 
Constrained  state  estimation  has  also  been  studied  from 
a  game-theoretical  point  of  view  (also  called  the  mini¬ 
max  or  Hg^  estimation)  in  [23]. 

In  this  paper,  the  constrained  optimization  is  solved 
with  the  Lagrangian  multiplier,  leading  to  a  closed-form 
solution  for  linear  constraints  and  an  iterative  solution 
for  nonlinear  constraints.  In  the  latter  case,  we  present  a 
method  that  allows  for  the  use  of  second-order  nonlinear 
state  constraints  exactly.  The  method  can  provide  better 
approximation  to  higher  order  nonlinearities.  The  new 
method  is  based  on  a  computational  algorithm  that 
iteratively  finds  the  Lagrangian  multiplier.  The  use  of  a 
second-order  constraint  versus  linearization  is  a  tradeoff 
between  reducing  approximation  errors  to  higher-order 
nonlinearities  and  keeping  the  problem  computationally 
tractable. 

A  nonlinear  constraint  can  be  approximated  with 
linear  constraints  in  a  piecewise  fashion.  By  judicious 
selection  of  the  number  of  linear  segments  and  their 
placement  (i.e.,  the  point  around  which  to  linearize),  a 
reasonably  good  performance  can  be  expected.  In  the 
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limit,  a  nonlinear  function  is  represented  by  a  piece- 
wise  function  composed  of  an  infinite  number  of  linear 
segments.  This  naturally  leads  to  the  use  of  nonlinear 
constraints.  As  such,  the  proposed  nonlinear  constrained 
solution  for  curved  roads  is  not  only  more  accurate  but 
also  less  complicated  in  implementation  than  a  piece- 
wise  linearized  constrained  approach,  to  be  shown  later 
in  simulation  examples. 

Although  the  main  results  are  restricted  to  state 
equality  constraints,  it  can  be  extended  to  inequality 
constraints.  According  to  [24],  the  inequality  constraints 
can  be  checked  at  each  time  step  of  filtering.  If  the  in¬ 
equality  constraints  are  satisfied  at  a  given  time  step,  no 
action  is  taken  since  the  inequality  constrained  problem 
is  solved.  If  the  inequality  constraints  are  not  satisfied 
at  a  given  time  step,  then  the  constrained  solution  is 
applied  to  enforce  the  constraints. 

The  paper  is  organized  as  follows.  Section  2  presents 
linearly  constrained  state  estimation  for  fusion  of  tracks 
with  linear  road  segments.  Section  3  presents  an  iter¬ 
ative  solution  for  fusion  of  tracks  with  nonlinear  road 
segments.  In  both  cases,  geometric  interpretations  of  the 
solutions  are  provided  for  special  cases.  In  Section  4, 
computer  simulation  results  are  presented  to  illustrate 
the  algorithms.  Finally,  Section  5  provides  concluding 
remarks  and  suggestions  for  future  work. 

2.  FUSION  OF  TRACKS  WITH  LINEAR  ROAD 
SEGMENTS 

When  a  road  segment  is  straight,  it  can  be  mod¬ 
eled  as  a  linear  state  constraint.  In  this  section,  we  first 
summarize  the  results  for  linearly  constrained  state  es¬ 
timation  [24]  as  an  approach  to  fusion  of  tracks  with 
linear  road  segments.  We  then  show  that  this  linearly 
constrained  state  estimation  is  equivalent  to  use  of  con¬ 
straints  as  measurements  in  state  update.  Finally,  we 
provide  a  simple  geometric  interpretation  of  the  linearly 
constrained  state  estimation  for  track-to-road  fusion. 

2.1 .  Linearly  Constrained  State  Estimation  for  Track-to- 
Road  Eusion 

Consider  a  linear  time-invariant  discrete-time  dy¬ 
namic  system  together  with  its  measurement  as 

=Ax^+Bu^-t-Wi  (la) 

=  (lb) 

where  the  subscript  k  is  the  time  index,  x  is  the  state 
vector,  u  is  a  known  input,  y  is  the  measurement,  and 
w  and  V  are  state  and  measurement  noise  processes, 
respectively.  It  is  implied  that  all  vectors  and  matrices 
have  compatible  dimensions,  which  are  omitted  for 
simplicity. 

The  goal  is  to  find  an  estimate  denoted  by  x^;.  of 
given  the  measurements  up  to  time  k  denoted  by  1^  = 
{yo,...,y^}.  Under  the  assumptions  that  the  state  and 
measurement  noises  are  uncorrelated  zero-mean  white 
Gaussian  with  w~A{0,Q}  and  v~A{0,R}  where  Q 


and  R  are  positive  semi-definite  covariance  matrices, 
the  Kalman  filter  provides  an  optimal  estimator  in  the 
form  of  x^  =  £{x^  1 1^}  [3].  Starting  from  an  initial  esti¬ 
mate  Xq  =  £{xo}  and  its  estimation  error  covariance  ma¬ 
trix  Pg  =  ^{(xq  —  Xq)(xq  —  Xg)^}  where  the  superscript  ^ 
stands  for  matrix  transpose,  the  Kalman  filter  equations 
specify  the  propagation  of  x^  and  over  time  and  the 


update  of  x^  and  P^.  by  measurement  y^  as 

(2a) 

P,,i=AP,A^  +  Q  (2b) 

=Xi^i  +K^^i(y^^i  -Cx^^i)  (2c) 

P,,i  =(I-K,,iC)P,,i  (2d) 

=P,,iC^(CP,C^+R)-i  (2e) 


where  x^^j  and  are  the  predicted  state  and  predic¬ 
tion  error  covariance,  respectively. 

Now  in  addition  to  the  dynamic  system  of  (1),  we 
are  given  the  linear  state  constraint  equation 

Dx^  =  d  (3a) 

where  D  is  a  known  constant  matrix  of  full  rank,  d  is 
a  known  vector,  and  the  number  of  rows  in  D  is  the 
number  of  constraints,  which  is  assumed  to  be  less  than 
the  dimension  of  states.  If  D  is  a  square  matrix,  the  state 
is  fully  constrained  and  can  thus  be  solved  by  inverting 
(3a).  Although  no  time  index  is  given  to  D  and  d  in  (3a), 
it  is  implied  that  they  can  be  time-dependent,  leading  to 
piecewise  linear  constraints. 

The  information  about  a  target  traveling  along  a  lin¬ 
ear  road  segment  is  well  modeled  by  (3a)  and  illus¬ 
trated  in  Fig.  1.  As  shown,  the  road  is  specified  by  the 
orientation  6  defined  as  the  angle  of  its  normal  vector 
n  relative  to  the  x-axis  and  the  distance  to  the  origin 
r.  The  unit  vectors  pointing  along  the  road  and  per¬ 
pendicular  to  the  road  are  given  by  /r  =  [— sind,cosd]^ 
and  n  =  [cosd,sin0]^,  respectively.  Clearly,  a  target  at 
position  p  =  [x,yY  with  velocity  v  =  [x,yY  satisfies  the 
linear  constraints  p^n  =  r  and  v^n  =  0.  These  two  equa¬ 
tions  can  be  easily  put  together  into  the  format  of  (3a) 
with  the  corresponding  D  and  d  given  below. 


'cos0 

0 

sind 

0  ■ 

D  = 

0 

cosd 

0 

sin0. 

,  d  = 

.0. 

(3b) 

The  constrained  Kalman  filter  according  to  [24] 
is  constructed  by  directly  projecting  the  unconstrained 
state  estimate  Xj.  onto  the  constrained  surface  5  =  {x : 
Dx  =  d}.  It  is  formulated  as  the  solution  to  the  problem 

X  =  argmin(x  —  x)^W(x  —  x)  (4) 

xg5 

where  W  is  a  symmetric  positive  definite  weighting 
matrix.  The  time  index  subscript  k  is  dropped  from 
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variables  in  (4)  for  simplicity.  When  W  =  I,  the  cost 
function  of  (4)  is  the  standard  least  squares  formulation. 
If  W  is  chosen  based  on  the  estimation  error  covariance 
matrix  P,  it  becomes  the  weighted  least  squares  solution. 

Derived  using  the  Lagrangian  multiplier  technique  in 
Appendix  A,  the  solution  to  the  constrained  optimiza¬ 
tion  in  (4)  is  given  by  [24] 

i  =  i-W^*D^(DW^^D^r'(Dx-d).  (5) 

Several  interesting  statistical  properties  of  the  con¬ 
strained  Kalman  filter  are  presented  in  [24].  This  in¬ 
cludes  the  fact  that  the  constrained  state  estimate  as 
given  by  (5)  is  an  unbiased  state  estimate  for  the  sys¬ 
tem  in  (1)  subject  to  the  constraint  in  (3a)  for  a  known 
symmetric  positive  definite  weighting  matrix  W.  Fur¬ 
thermore  when  W  =  the  constrained  state  estimate 
has  a  smaller  error  covariance  than  that  of  the  uncon¬ 
strained  state  estimate,  and  it  is  actually  the  smallest  for 
all  constrained  Kalman  filters  of  this  type. 

2.2.  Track-to-Road  Fusion  Architectures^ 

According  to  (4),  the  fusion  of  a  target  track  (an 
unconstrained  state  estimate)  with  a  road  segment 


’This  subsection  is  added  based  on  the  editor’s  comments. 


(a) 

Fig.  2.  Track-to-i 


represented  by  a  surface  in  the  state  space  S  is  cast  as 
a  constrained  least  squares  optimization  problem,  yield¬ 
ing  the  constrained  solution  x  and  its  estimation  error 
covariance  P^.  This  leads  to  two  possible  implementa¬ 
tion  schemes.  One  is  the  open-loop  architecture  with¬ 
out  feedback  as  shown  in  Fig.  2(a)  and  the  other  is 
the  closed-loop  architecture  with  feedback  as  shown  in 
Fig.  2(b). 

In  the  open-loop  architecture  of  Fig.  2(a),  the  uncon¬ 
strained  solution  can  be  used  to  help  select  the  proper 
road  constraints  prior  to  fusion  and  the  fused  solution 
may  be  further  used  to  refine  road  constraints  for  future 
target  movements.  However,  the  fused  state  is  not  fed 
back  to  the  unconstrained  tracker. 

In  contrast,  the  closed-loop  architecture  of  Fig.  2(b) 
feeds  back  the  fused  state  to  the  unconstrained  Kalman 
tracker  (i.e.,  to  replace  the  state  with  the  fused  state). 
This  has  the  advantage  of  keeping  the  one-step-ahead 
prediction  closely  aligned  to  the  road  estimates. 

There  are  several  issues  to  trade  off  when  making 
the  choice  of  one  architecture  versus  the  other.  The 
closed-loop  scheme  needs  to  alter  the  unconstrained 
filter  and  its  implementation  therefore  requires  inter¬ 
nal  access.  Further,  a  two-way  data  link  may  be  nec¬ 
essary  if  the  tracker  and  the  fusion  center  are  not 
co-located. 

The  open-loop  architecture  is  simple  and  can  be 
retrofitted  into  existing  trackers  as  an  add-on  module 
without  changes  to  the  trackers.  Since  the  tracks  are  ob¬ 
tained  without  constraints,  it  can  easily  switch  between 
off-road  and  on-road  operations.  It  is  particularly  useful 
for  cases  where  no  up-to-date  accurate  road  map  (e.g., 
the  latest  satellite  imagery)  is  available  to  individual  sen¬ 
sors  but  at  a  fusion  center.  In  this  paper,  the  open-loop 
scheme  is  implemented  in  the  simulation  examples  pre¬ 
sented  in  Section  4. 

As  shown  in  Fig.  2,  an  important  step  leading  to  the 
track-to-road  fusion  is  the  road  constraint  generation.  It 
consists  of  two  major  parts,  namely,  creating  an  analytic 


(b) 


fusion  architectures. 
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representation  for  a  given  road  segment  and  selecting 
the  correct  road  segment(s)  for  fusion. 

In  a  digital  geographic  database,  road  network  is 
stored  as  a  series  of  waypoints,  a  layer  of  the  vector  map 
(VMAP).  The  waypoints  are  typically  extracted  from 
survey  data  and  aerial  imagery  among  others  and  the 
density  or  spacing  of  waypoints  is  determined  by  the 
map  resolution,  which  may  be  nonuniform.  Although 
local  survey  data  may  contain  the  radius  and  turn  center 
of  a  curved  road  segment,  the  waypoints  themselves  do 
not  define  the  functions  representing  the  road.  To  apply 
the  road-constrained  optimal  fusion  method,  it  is  nec¬ 
essary  to  generate  the  constraint  function  based  on  the 
waypoints  in  the  database.  The  most  typical  approach 
would  use  linear  segments  to  connect  the  waypoints  as 
evident  from  Google  Map,  MapQuest,  or  MSN  Maps 
when  zooming  in.  The  waypoint  connections  defines  a 
line  representing  road,  which  can  be  a  simple  line  con¬ 
necting  two  waypoints  or  a  tangent  passing  through  a 
waypoint.  Alternately,  a  spline  (a  piecewise  polynomial 
function)  can  be  used  to  define  the  road  in  between 
the  points,  leading  to  a  nonlinear  function  defining  the 
road. 

Ideally,  the  number  of  waypoints  used  to  define  the 
road  is  generated  such  that  the  maximum  error  between 
the  actual  road  and  the  mathematical  model  for  the 
road  (linear  segments,  spline,  etc.)  is  less  than  some 
allowable  value.  However,  waypoints  in  most  digital 
maps  are  pre- determined  and  fixed.  Depending  on  the 
map  resolution  and  sensor  accuracy,  when  the  error 
associated  with  the  constraints  becomes  larger  than 
the  error  in  the  sensor  measurement,  the  benefits  of 
using  such  constraints  diminish.  It  is  therefore  desired 
to  have  a  road  modeling  system  that  generates  the 
waypoints  to  support  “adaptive  sampling”  so  that  the 
error  between  the  road  and  the  road  model  is  always 
less  than  some  limit.  The  use  of  an  analytic  nonlinear 
representation,  rather  than  fixed  waypoints  with  linear 
segments,  is  a  possible  way  toward  adaptive  sampling 
and  resampling. 

The  second  aspect  of  the  road  constraint  generation 
shown  in  Fig.  2  is  constraint  selection,  which  identifies 
which  road  the  target  is  on  and  the  closest  waypoints 
on  the  road  and  then  produces  the  constraint  function 
for  those  points  of  the  road.  Similar  to  the  problem 
of  target  tracking  with  measurement  origin  uncertain¬ 
ties  where  data  association  is  applied  prior  to  mea¬ 
surement  updating,  the  track-to-road  fusion  necessitates 
road  constrained  data  association  (RCDA)  especially 
with  closely  spaced  roads  and  around  intersections.  This 
association  can  be  either  measurement-based  or  pre¬ 
dicted  state-based  and  a  data  history  may  be  needed 
to  ascertain  the  winning  hypothesis. 

For  an  identified  road,  it  then  comes  to  select  a  piece- 
wise  constraint  model.  Without  pre-determined  analytic 
models  available  for  the  road  segment,  it  is  possible  to 


perform  on-line  synthesis.  For  example,  from  two  clos¬ 
est  waypoints  to  a  measurement,  a  line  representation 
can  be  computed  for  those  points.  Or  a  spline  repre¬ 
sentation  of  the  road  can  be  computed  for  the  nearest 
three  points  of  the  digital  map.  A  nonlinear  represen¬ 
tation  (from  the  spline  for  instance)  further  allows  for 
piecewise  linearization  with  the  point  for  the  lineariza¬ 
tion  chosen  near  the  measurement  or  near  the  estimated 
track  state.  Iterative  linearization  can  be  used  to  refine 
the  linearized  constraints  if  necessary. 

For  lines  between  fixed  waypoints  or  piecewise  lin¬ 
earized  segments  from  a  nonlinear  model,  the  linear 
constrained  optimization  method  of  this  section  can  be 
applied.  For  curved  roads,  the  nonlinear  optimization 
method  presented  in  Section  3  can  be  used  advanta¬ 
geously  when  a  nonlinear  representation  of  a  road  is 
available. 

The  aspects  of  constraint  modeling  and  selection 
are  not  further  discussed  in  this  paper.  Another  impor¬ 
tant  issue  that  is  not  addressed  either  in  this  paper  is 
possible  errors  in  digital  maps  such  as  bias  and  mis- 
orientation  for  linear  road  segments  and  erroneous  ra¬ 
dius  and  turn  center  for  curved  segments.  We  leave  it  for 
future  treatment  but  focus  on  fusion  methodology  in  this 
paper. 

2.3.  Linear  Road  Constraint  as  Pseudo  Measurement 

As  described  above,  the  linear  constrained  estimator 
(5)  can  be  obtained  by  different  methods.  It  is  shown 
in  this  section  that  it  is  also  equivalent  to  the  solution 
where  the  linear  state  constraints  are  considered  as 
pseudo  measurements. 

For  the  linear  time-invariant  discrete-time  dynamic 
system  (la)  and  its  measurement  (lb),  consider  the  lin¬ 
ear  state  constraint  (3)  as  an  additional  measurement 
to  the  system,  which  can  be  used  to  perform  the  fil¬ 
ter  measurement  update  (2c)  and  (2d)  right  after  (lb) 
without  the  filter  time  propagation  (2a)  and  (2b)  (i.e., 
stay  the  same).  To  apply  (2),  we  identify  the  following 
equivalence: 

C  =  D,  R  =  0,  y,  =d.  (6) 

Consider  (x['\P®)  as  the  constrained  state  and  co- 
variance  after  the  ith  iteration  update  with  the  con¬ 
straints  at  time  k.  With  this  notation,  (x['\P®)  =  (Xj.,Pj.) 
estimate  for  /  =  0  is  the  unconstrained  state  estimate  and 
covariance  at  time  k.  The  Kalman  filter  gain  is  given 
by 

k0+  1)  =  p®d^(DP®D^)-'  .  (7) 

The  updated  state  and  error  covariance  becomes: 

-r  P®D^(DP® D^)- ^  (d  -  Di® )  (8) 

pO+i)  ^  p(0  _  p®d^(DP®D^)-1DP®.  (9) 
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If  we  choose  W  =  (P®)  \  (8)  becomes 

+  W-‘D^(DW-'D^)-Hd  -  Di®) 

(10a) 

=  i®  -  W-‘D^(DW-‘D^)-HDi®  -  d) 

(10b) 

=  -  W-*D^(DW-*D^)-*(Di^  -  d) 

(10c) 

which  is  exactly  the  same  as  the  solution  given  by  (5). 

This  equivalence  affords  a  possible  way  to  incorpo¬ 
rate  uncertainty  in  road  modeling  such  as  bias,  width, 
and  mis-orientation  through  pseudo  measurement  error 
covariance  matrix  R.  In  the  ideal  case  where  roads  are 
assumed  to  be  known  perfectly,  this  R  is  set  to  zero.  In¬ 
equality  constraint  is  another  way  to  handle  uncertainty 
if  errors  are  within  certain  known  bounds.  Furthermore, 
when  the  track-to-road  fusion  is  based  on  optimization 
with  a  least-squares  criterion,  it  is  possible  to  introduce 
weightings  to  account  for  directional  errors  given  by 
covariance  matrices  of  the  track  and/or  the  road. 

2.4.  Geometric  Interpretation 

Assume  that  the  state  dimension  is  n  and  the  number 
of  linear  constraints  is  m  <  n.  For  x  G  K",  the  constraint 
5  =  {x  :  Dx  =  d}  constitutes  a  surface  in  K".  It  is  shown 
in  Appendix  B  that  for  the  case  where  W  =  I,  the  linear 
constrained  estimation  (5)  is  the  orthogonal  projection 
of  the  unconstrained  estimate  onto  the  constraining  sur¬ 
face.  This  offers  a  geometric  interpretation  and  provides 
a  theoretical  justification  of  the  intuitive  practice  of  find¬ 
ing  a  point  along  the  road  that  is  of  the  shortest  distance. 

The  theory  still  holds  for  W  I.  The  proof  is  given 
in  Appendix  C.  The  results  presented  in  this  and  pre¬ 
vious  sections  complement  the  work  of  [24],  providing 
an  interesting  geometric  interpretation  to  the  linear  con¬ 
strained  estimation  by  estimate  projection. 

3.  FUSION  OF  TRACKS  WITH  NONLINEAR  ROAD 
SEGMENTS 

When  a  road  segment  is  curved,  it  can  be  modeled 
as  a  nonlinear  state  constraint.  In  this  section,  we  first 
analyze  the  linearizing  approach  and  the  associated  con¬ 
straint  approximation  error.  We  then  present  an  iterative 
solution  to  a  second  order  state  constraint.  Finally,  we 
offer  a  geometric  interpretation  of  the  solution  under  a 
circular  constraint  and  outline  a  simple  approach  to  a 
more  general  second  order  state  constraint  problem  of 
practical  significance. 

3.1.  Approximation  Errors  in  Constraint  Linearization 

To  deal  with  nonlinearity,  a  simple  approach  is  to 
project  the  unconstrained  state  estimate  onto  linearized 


state  constraints.  Once  the  constraints  are  linearized, 
the  results  presented  in  the  previous  section  for  linear 
cases  can  be  applied.  However,  linearization  introduces 
constraint  approximation  error,  which  is  a  function  of 
the  nonlinearity  and,  more  importantly,  of  the  point 
around  which  the  linearization  takes  place.  This  may 
lead  to  an  undesired  divergence  problem  as  analyzed 
below. 

Consider  the  nonlinear  state  constraint  of  the  form 
g(x)  =  d.  (11) 

We  can  expand  the  nonlinear  state  constraints  about 
a  constrained  state  estimate  x  and  for  the  /th  row  of  (1 1), 
we  have 

g,.(x)  -  d.  =  g.(x)  -(-  gKx)^(x  -  x) 

+  ^(x-i)^g"(i)  +  (x-i)  +  ----d,  =  0 

(12) 

where  the  superscripts '  and  "  denote  the  first  and  second 
partial  derivatives. 

Keeping  only  the  first-order  terms  as  suggested  in 
[24],  some  rearrangement  leads  to 

g'(x)^x  «  d  -  g(x) -t  g'(x)^x  (13) 

where  g(x)  =  d  =  [...d,...]^,  and  g'(x)  = 

[. ..g[(x)...].  An  approximate  linear  constraint  is  there¬ 
fore  formed  by  replacing  D  and  d  in  (3)  with  g'(x)^  and 
d  —  g(x)  +  g'(x)^x,  respectively. 

Fig.  3  illustrates  this  linearization  process  and  iden¬ 
tifies  possible  errors  associated  with  linear  approxima¬ 
tion  of  a  nonlinear  state  constraint.  As  shown,  a  previous 
constrained  state  estimate  x^  lies  somewhere  on  the  con¬ 
strained  surface  but  is  away  from  the  true  state  x.  The 
projection  of  the  unconstrained  state  estimate  x  onto  the 
approximate  linear  state  constraint  produces  the  current 
constrained  state  estimate  x"^ ,  which  is  however  subject 
to  the  constraint  approximation  error.  Clearly,  the  fur¬ 
ther  away  is  x^  from  x,  the  larger  is  the  approximation- 
introduced  error.  More  critically,  such  an  approximately 
linear  constrained  estimate  may  not  satisfy  the  original 
nonlinear  constraint  specified  in  (11).  It  is  therefore  de¬ 
sired  to  reduce  this  approximation-introduced  error  by 
including  higher-order  terms  while  keeping  the  prob¬ 
lem  computationally  tractable.  One  possible  approach 
is  presented  in  the  next  section. 

As  discussed  in  Section  2.2,  when  waypoints  of 
a  digital  map  are  used  to  construct  linear  constraints 
directly,  their  modeling  error  is  related  to  a  large  extent 
to  the  coarseness  of  waypoints,  which  is  determined  in 
turn  by  the  map  resolution. 

Working  with  an  analytic  road  model,  simply  curve- 
fitted  from  fixed  waypoints  for  instance,  provides 
the  opportunity  for  possible  “iterated  linearization”  or 
“adaptive  sampling”  so  as  to  maintain  small  uniform  lin¬ 
earization  errors.  As  shown  in  Fig.  3,  when  the  lineariza¬ 
tion  point  is  far  away  from  the  true  state,  the  lineariza- 
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X  True  State  x  Previous  Constrained  State  Estimate 

X  Unconstrained  State  Estimate  Current  Constrained  Sate  Estimate 

Fig.  3.  Errors  in  linear  approximation  of  nonlinear  state  constraints. 


tion  is  poor.  In  this  particular  case,  the  linearization 
point  is  the  predicted  state  x^,  which  happens  to  be  on 
road.  However,  due  to  target  motion,  this  predicted  state 
is  offset  from  the  true  state  x.  The  linear  constrained 
state  estimate  x"^  is  now  “closer”  to  the  true  state  than 
the  predicted  one  and  can  be  used  to  re-linearize  the 
function  as  done  in  an  iterated  extended  Kalman  filter. 
This  iterated  linearization  may  reduce  linearization  error 
in  a  sense  but  cannot  guarantee  a  smaller  state  estima¬ 
tion  error  because  the  linear  constrained  state  estimate 
and  its  iterations  may  not  always  fall  onto  the  road. 

At  a  first  glance,  a  curved  road  can  be  well  approxi¬ 
mated  with  a  sufficient  number  of  waypoints  where  the 
linearization  points  are  critically  placed  (i.e.,  the  way- 
point  sample  rate  is  sufficient  to  keep  the  error  between 
the  road  and  the  road  model  small).  In  the  limit,  a  piece- 
wise  linear  approximation  converges  to  a  continuous 
function;  and  the  direct  use  of  a  nonlinear  constraint 
itself,  rather  than  its  approximation,  becomes  natural. 

In  practical  cases,  however,  only  a  limited  number  of 
waypoints  are  available.  For  sharp  turns,  linear  approx¬ 
imation  errors  dominate.  As  shown  later  in  Section  4.1, 
the  selection  of  a  linear  segment  and  in  particular  the 
transition  from  one  segment  to  another  is  a  rather  in¬ 
volved  process.  On  the  other  hand,  the  track-to-road 
fusion  is  considerably  simplified  with  nonlinear  con¬ 
strained  optimization  as  described  below. 

3.2.  Iterative  Solution  to  Second-Order  Constraints 

Naturally  formed  roads  tend  to  have  more  bends 
and  turns  of  irregular  shapes  (high  nonlinearity).  Even 
highways  have  to  follow  terrain  contours  when  crossing 
mountains.  Locally,  however,  it  suffices  to  represent  a 
curved  road  segment  by  a  second-order  state  constraint 
function  as 

X 
1, 

=  x^Mx -f  m^x  +  x^m -t  OTf)  =  0  (14) 

which  can  be  viewed  as  a  second-order  approximation 
to  an  arbitrary  nonlinearity  in  a  digital  terrain  map. 


/(X)  =  [x^  1] 


M 


m 

mn 


Similar  to  (4),  we  can  formulate  the  projection  of 
an  unconstrained  state  estimation  onto  a  nonlinear  con¬ 
straint  surface  as  the  constrained  least-squares  optimiza¬ 
tion  problem 

X  =  arg  min(z  —  Hx)^(z  —  Hx)  (15a) 

X 

subject  to  /(x)  =  0.  (15b) 

If  we  let  W  =  H^H  and  z  =  Hx,  the  formulation  in 
(15)  becomes  the  same  as  in  (4).  In  a  sense,  (15)  is  a 
more  general  formulation  because  it  can  also  be  inter¬ 
preted  as  a  nonlinear  constrained  measurement  update 
or  a  projection  in  the  predicted  measurement  domain. 

The  solution  to  the  constrained  optimization  (15) 
can  be  obtained  again  using  the  Lagrangian  multiplier 
technique,  which  is  detailed  in  Appendix  D,  as 

i  =  G-*V(I -t  AS^S)-ie(A)  (16a) 


q(X}  = 


efWaf 
(1  +  Aa2)2 


••.(AX: 


+ 


-t  wia  =  0 


(16b) 

where  G  is  an  upper  right  diagonal  matrix  resulting  from 
the  Cholesky  factorization  of  W  =  as 


W  =  G^'G  (16c) 


V,  an  orthonormal  matrix,  and  S,  a  diagonal  matrix  with 
its  diagonal  elements  denoted  by  are  obtained  from 
the  singular  value  decomposition  (SVD)  of  the  matrix 
LG-i  as 

LG-‘  =  USV^'  (16d) 


where  U  is  the  other  orthonormal  matrix  of  the  SVD 
and  L  results  from  the  factorization  M  =  L^^L,  and 

e(A)  =  [...e,.(A),...]^  =  V2’(G2')-TH2'z-Am) 

(16e) 

t  =  [...t,....]2' =  v2'(G2')-'m.  (16f) 


As  a  nonlinear  equation  in  A,  it  is  difficult  to  find  a 
closed-form  solution  in  general  for  the  nonlinear  equa¬ 
tion  g(A)  =  0  in  (16b).  Numerical  root-finding  algo¬ 
rithms  may  be  used  instead.  For  example,  the  Newton’s 
method  is  used  below.  Denote  the  derivative  of  q{\)  with 
respect  to  A  as 


^(A)  =  2V 


G(A)g(1  +  Ao-f)o-f  -  g- (A)crf 
(1  +  A(t2)3 


I  +  Ag?)-g,(A)t,(T2 

^  (1  +  Acr2)2 

I  * 


(17a) 


where 

e  =  [...e,....]2'  =  -v2'(G2')-'m.  (17b) 


Then  the  iterative  solution  for  A  is  given  by 


A 


k+\ 


q(\) 


starting  with  Aq  =  0. 


(18) 
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The  iteration  stops  when  |A^+i  —  Xk\  <  T’  a  given 
small  value  or  the  number  of  iterations  reaches  a  pre¬ 
specified  number.  Then  bringing  the  converged  La- 
grangian  multiplier  A  back  to  (16a)  provides  the  con¬ 
strained  optimal  solution. 

Now  consider  the  special  case  where  W  =  H^H, 
z  =  Hx,  and  m  =  0,  that  is,  a  quadratic  constraint  on  the 
state.  Under  these  conditions,  t  =  0  and  e  is  no  longer 
a  function  of  A  so  its  derivative  relative  to  A  vanishes, 
e  =  0.  The  quadratic  constrained  solution  is  then  given 

by  .  .  . 

x  =  (W-t-AM)-iWx  (19a) 

where  the  Lagrangian  multiplier  A  is  obtained  iteratively 
as  in  (18)  with  the  corresponding  q(X)  and  ^(A)  given 
by 


i  ^ 

(19b) 

qiX)=  2  V  V 

^  (1  +  Xaf)^ 

(19c) 

The  solution  of  (19)  is  also  called  the  constrained 
least  squares  [17;  pp  765-766],  which  was  previously 
applied  for  the  joint  estimation  of  angles  of  arrival  and 
calibration  of  channel  biases  of  a  linear  array  [29]. 
Similar  techniques  have  been  used  for  the  design  of 
filters  for  radar  applications  [1]  and  in  robust  minimum 
variance  beamforming  [14].  When  M  =  0,  the  constraint 
in  (14)  degenerates  to  a  linear  one.  The  constrained 
solution  is  still  valid.  However,  the  iterative  solution 
for  finding  A  is  no  longer  applicable  but  a  closed-form 
solution  is  available  instead  as  given  in  (5). 

3.3.  Geometric  Interpolation  for  Simple  Cases 

Consider  a  simple  example  where  a  target  travels 
along  a  circle.  For  this  case,  in  fact,  a  closed-form 
solution  can  be  derived.  Assume  that  W  =  I,  M  =  I, 
m  =  0,  and  =  —  r^.  Let  p  be  the  position  components 
of  the  state  x,  to  which  the  constraint  is  applied.  The 
nonlinear  constraint  can  be  equivalently  written  as; 

P^P  =  r^.  (20) 


Bringing  the  solution  for  A  in  (23)  back  to  (21)  gives; 

This  indicates  that  for  this  particular  case  with  a  cir¬ 
cular  constraint,  the  constraining  results  in  normaliza¬ 
tion. 

This  further  suggests  a  simple  solution  for  some 
practical  applications.  When  a  target  is  traveling  along 
a  circular  path  (or  approximately  so),  one  can  first 
find  the  equivalent  center  of  the  circle  around  which 
to  establish  a  new  coordinate  system.  Then  express 
the  unconstrained  solution  in  the  new  coordinate  and 
normalize  it  as  the  constrained  solution.  Finally  convert 
it  back  to  the  original  coordinates.  For  non-circular  but 
second-order  paths,  eigenvalue-based  scaling  may  be 
effected  following  coordinate  translation  and  rotation 
in  order  to  apply  this  circular  normalization.  Reverse 
operations  are  in  order  to  transform  back  to  the  original 
coordinates.  For  applications  of  high  dimensionality,  the 
scalar  iterative  solution  of  (17)  may  be  more  efficient. 

4.  SIMULATION  RESULTS 

In  this  section,  two  simulation  examples  are  pre¬ 
sented  in  the  context  of  on-road  ground  vehicle  tracking. 
The  first  example  compares  linearized  and  nonlinear 
constraining  schemes  for  a  simple  tracker  and  the  sec¬ 
ond  example  compares  unconstrained  and  constrained 
IMM  trackers. 

4.1.  Linearized  versus  Nonlinear  Constraints  for  a 
Simple  Tracker 

In  this  simulation  example,  a  ground  vehicle  is  as¬ 
sumed  to  travel  along  a  circular  road  segment  as  shown 
in  Fig.  3.  The  turn  center  is  chosen  as  the  origin  of  the 
x-y  coordinates  and  the  turn  radius  is  r  =  100  m.  The 
target  maintains  a  constant  turn  rate  of  5.7296  deg/s 
with  an  equivalent  linear  speed  of  10  m/s.  The  initial 
state  is 

x^^o  =  [x  X  y  yY  =  [100  m  0  m/s  0  m  10  m/s]^. 


The  quadratic  constrained  estimate  given  in  (19a) 
becomes; 


p  =  (W-(-AM)-'Wp  =  (l-tA)-^p  (21) 


where  A  is  the  Lagrangian  multiplier. 
Bringing  (21)  back  to  (20)  gives; 


“7^ 

P  P 


-  X  7 

P  \  P 

1  +  Ay  1  +  A 


(22) 


One  solution  for  A  is; 

^  ^  TFp  ^  ^  Mlk  _  1 

r  r 


(23) 


where  ||  •  [2  stands  for  the  Lj-norm  or  length  for  the 
vector. 


(25) 

The  vehicle  is  tracked  by  a  radar  sensor  with  a  sam¬ 
pling  interval  of  T  =  1  s.  The  sensor  provides  position 
measurements  of  the  vehicle  as 


yk  = 


1  0  0  0' 
.0  0  10. 


^k  +  '^k 


(26) 


where  the  measurement  error  v~A(0,R)  is  a  zero- 
mean  Gaussian  noise,  independent  in  the  x-  and  y- 
axis.  The  covariance  matrix  R  =  diag([(T^  uses  the 
particular  values  of  =  (t^  =  7  m  in  the  simulation.  To 
use  the  position  measurement  model  (26),  it  is  assumed 
that  the  radar-produced  measurements  in  a  polar  frame 
are  converted  to  the  Cartesian  frame  and  the  errors 
associated  with  the  conversion  are  ignored. 
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Fig.  4.  Sample  trajectories  for  linear  constrained  Kalman  filter. 


The  radar  implements  a  simple  tracker  based  on  the 
following  discrete-time  second-order  kinematic  model 
(nearly  constant  velocity) 


-1 

T 

0 

0- 

r  ^ 

2^ 

0  - 

0 
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0 

0 

T 

0 

Xa-  +  1  = 

0 

0 

1 

T 

0 

17^2 

2-' 

.0 

0 

0 

1. 

.  0 

T  . 

(27) 

where  the  process  noise  w  ~  N(0,  Q)  is  also  a  zero-mean 
Gaussian  noise,  independent  of  the  measurement  noise 
V.  The  covariance  matrix  Q  =  diag([CT|  ct|])  uses  the 

particular  values  of  tr-  =  ct-  =  0.32  m/s^  in  the  simula¬ 
tion. 

When  represented  in  a  Cartesian  coordinate  system, 
a  target  traveling  along  a  curved  road  is  certainly  subject 
to  acceleration  in  both  the  x-  and  y-axis.  However,  no 
effect  is  made  in  this  simulation  to  optimize  the  tracker 
for  maneuver  but  merely  to  select  Q  and  the  initial 
conditions  so  as  to  focus  on  constraining  the  estimates. 
The  use  of  an  IMM  filter  [5]  with  “coordinated  turn” 
models  will  be  presented  next  in  Section  4.2.  The  initial 
state  is  selected  to  be  the  same  as  the  true  state,  i.e., 
Xq  =  Xq  for  this  example,  again  to  focus  on  the  aspect 
of  track-to-road  fusion,  not  on  that  of  tracker  design. 
The  initial  estimation  error  covariance  is  selected  to  be 

Pq  =  diag([52  m^  1^  (m/s)^  5^  m^  1^  (m/s)^]). 

(28) 

Fig.  4  shows  sample  trajectories  of  the  linear  con¬ 
strained  Kalman  filter.  There  are  5  curves  and  2  series 


of  data  points  in  the  figure.  The  true  state  is  represented 
by  a  series  of  dots  (•)  at  consecutive  sampling  instants, 
which  is  plotted  on  the  solid  line  being  the  road  seg¬ 
ment.  The  corresponding  measurements  are  a  series  of 
circles  (o). 

The  estimates  of  the  unconstrained  Kalman  filter  are 
shown  as  the  connected  triangles  (A)  whereas  those 
of  linearly  constrained  Kalman  filters  are  shown  as  the 
connected  crosses  (x),  stars  (*),  and  pluses  (-f)  for  three 
linear  approximations  of  the  nonlinear  constraint  of  the 
curved  road,  respectively. 

In  the  first  approximation  (the  line  with  cross  x  la¬ 
beled  “linear  constraint  1”),  a  single  linearizing  point  at 
=  10°  is  chosen  to  cover  the  entire  curved  road,  where 
9  is  the  angle  made  relative  to  the  x-axis,  positive  in  the 
counter-clock  direction.  The  linearized  state  constraint 
at  9 1  can  be  written  as 


'cosdj 

0 

sin0j 

0  ■ 

X  = 

>' 

0 

COS0J 

0 

sin0[ . 

0. 

Although  all  estimates  are  faithfully  projected  by  the 
constrained  filter  onto  this  linear  constraint,  tangential 
to  the  curve  at  the  linearizing  point,  it  runs  away  from 
the  true  trajectory  and  the  resulting  errors  continue  to 
grow.  The  apparent  divergence  is  caused  by  the  choice 
of  linearization. 

In  the  second  approximation  (the  line  with  star  * 
labeled  “linear  constraint  2”),  two  linearizing  points  at 
9 1  =  15°  and  dj  =  80°  are  chosen  to  cover  the  curved 
road  with  two  linear  segments.  The  switching  point 
from  one  linear  segment  to  the  other  in  this  case  is 
at  0  =  45°.  As  shown,  the  estimates  are  projected  onto 
one  of  the  two  linear  segments.  Except  near  the  corner 
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linear  constrained  position  absoiute  errors 


Fig.  5.  Linear  constrained  position  errors  versus  time. 


where  the  two  linear  approximations  intersect  (which 
is  far  away  from  both  linearizing  points),  the  linear 
constrained  estimates  typically  outperform  the  uncon¬ 
strained  estimates  (closer  to  the  true  state).  This  is  better 
illustrated  in  Fig.  5  where  the  upper  plot  is  for  the  ab¬ 
solute  position  error  in  x  while  the  lower  plot  is  for  the 
absolute  position  error  in  y,  both  plotted  as  a  function 
of  time. 

Still  with  two  linearizing  points  and  the  same  switch¬ 
ing  point  at  0  =  45°,  the  third  approximation  (the  line 
with  plus-f  labeled  “linear  constraint  3”)  adjusts  lineariz¬ 
ing  points  to  =  20°  and  62  =  10° .  A  better  overall 
performance  is  achieved  as  shown  in  Fig.  5. 

It  is  clear  from  Fig.  4  that  a  nonlinear  constraint  can 
be  approximated  with  linear  constraints  in  a  piecewise 
fashion.  By  judicious  selection  of  the  number  of  linear 
segments  and  their  placement  (i.e.,  the  point  around 
which  to  linearize),  a  reasonably  good  performance 
can  be  expected.  In  the  limit,  a  nonlinear  function  is 
represented  by  a  piecewise  function  composed  of  an 
infinite  number  of  linear  segments.  This  naturally  leads 
to  the  use  of  nonlinear  constraints. 

Fig.  6  shows  sample  trajectories  of  the  nonlinear 
constrained  Kalman  filter.  There  are  2  curves  and  4 
series  of  data  points  in  the  figure.  The  true  state  is 
still  represented  by  a  series  of  dots  (•)  at  the  sampling 
instants,  which  is  plotted  on  the  solid  line  of  road 
segment.  The  corresponding  measurements  are  again  a 
series  of  circles  (o).  The  unconstrained  Kalman  filter 
is  shown  as  the  connected  crosses  (x)  whereas  the 
estimates  of  nonlinearly  constrained  Kalman  filters  are 
shown  as  a  series  of  pluses  (-(-)  and  stars  (*)  for  two 
implementations,  respectively. 


The  first  implementation  (the  series  of  pluses  -t) 
only  applies  the  nonlinear  constraint  to  the  position 
estimate  whereas  the  second  implementation  (the  se¬ 
ries  of  stars  *)  applies  constraints  to  both  the  position 
and  velocity  estimates.  In  fact,  we  encounter  a  hybrid 
(mixed)  linear  and  nonlinear  state  constraint  situation. 
The  constrained  position  estimate  is  given  by  (19)  for 
the  quadratic  case  (equivalent  to  (24)  for  a  circular 
road).  Since  the  velocity  direction  is  along  the  tangent 
of  the  road  curve,  the  constrained  velocity  estimate  is 
obtained  by  the  following  projection 


^constrained  ^^unconstrained 


(30) 


where  v  =  [i:  yY  is  the  estimated  velocity  vector  and 
/r=[— sin0  cos0]^  is  the  constrained  unit  direction 
vector  associated  with  the  constrained  position  at  9  = 
tan^^(y/x). 

In  the  present  simulation,  the  open-loop  architecture 
without  feedback  is  used.  In  this  implementation,  the 
unconstrained  estimation  error  covariance  is  not  mod¬ 
ified  after  the  constrained  estimate  is  obtained  using 
the  projection  algorithms  (19).  The  implementation  is 
therefore  pessimistic  (suboptimal)  in  the  sense  that  it 
does  not  take  into  account  the  reduction  in  the  estima¬ 
tion  error  covariance  brought  in  by  constraining.  One 
consequence  of  this  simplification  is  more  volatile  state 
estimates.  To  quantify  this  effect,  one  approach  is  to 
project  the  unconstrained  probability  density  function 
(i.e.,  a  normal  distribution  with  support  on  the  whole 
state  space)  onto  the  nonlinear  constraint.  Statistics  can 
then  be  calculated  from  the  constrained  probability  den¬ 
sity  function  with  the  constraint  as  its  support.  Again, 
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Fig.  7.  Nonlinear  constrained  position  errors  versus  time. 


the  resulting  error  ellipse  represented  by  the  covariance 
matrix  is  only  an  approximation  to  the  second  order. 
As  explained  in  Section  2.2,  the  open-loop  architecture 
without  feedback  has  many  merits  of  its  own  and  it 
here  provides  a  reference  point  for  fusion  architecture 
study. 

As  shown  in  Fig.  6,  both  the  nonlinear  constrained 
estimates  fall  onto  the  road  as  expected.  Overall  the 


position  and  velocity  constrained  estimates  are  better 
(closer  to  the  true  state)  than  the  position-only  con¬ 
strained  estimates.  This  is  illustrated  in  Fig.  7  where  the 
upper  plot  is  for  the  absolute  position  error  in  x  while 
the  lower  plot  is  for  the  absolute  position  error  in  y. 

A  Monte  Carlo  simulation  is  used  to  generate  the 
root  mean  sqnare  (RMS)  errors  of  state  estimation. 
The  results  are  based  on  a  total  of  100  runs  across  16 
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iterative  soiution  of  lagrangian  multiplier:  first  5  data  points 


Fig.  8.  Convergence  in  iterative  Lagrangian  multiplier. 


TABLE  I 

RMS  Estimation  Errors 


Estimators 

RMS  Estimation  Error 

Position  (m) 

Velocity  (m/s) 

Unconstrained 

8.4 

4.3 

Best  Linear  Constrained 

5.5 

2.5 

Nonlinear  Constrained 

1.8 

0.4 

updates  and  summarized  in  Table  I.  The  performance 
improvement  of  the  nonlinear  constrained  filter  over  the 
linearized  constrained  filter  is  demonstrated. 

Finally  for  this  simulation  example,  we  use  Fig.  8 
to  show  an  example  of  the  Lagrangian  multiplier  as  it 
is  calculated  iteratively  using  (19).  The  runs  for  five 
unconstrained  state  estimates  are  plotted  in  the  same 
figure  and  to  make  it  fit,  the  normalized  absolute  values 
of  A  are  taken.  As  shown,  starting  from  zero,  it  typically 
takes  4  iterations  for  the  algorithm  to  converge  in  the 
example  presented. 

4.2.  Unconstrained  Versus  Constrained  IMM  Trackers 

In  the  previous  example,  a  nearly  constant  velocity 
model  (27)  was  used  in  the  filter.  Obviously,  a  tracker 
that  uses  a  maneuvering  model  can  do  better  in  tracking 
a  turning  target.  However,  it  may  still  not  be  able  to 
produce  a  track  that  falls  on  road  all  the  time.  The  track- 
to-road  fusion  algorithm  described  in  this  paper  can  be 
applied  in  conjunction  with  a  maneuvering  target  tracker 
to  further  improve  target  state  estimation  as  illustrated 
in  the  following  simulation  example. 


An  IMM  filter  is  constructed  based  on  the  “coor¬ 
dinated  turn”  models.  For  a  ground  vehicle,  its  wide 
turning  maneuver  is  reasonably  well  modeled  by  a  co¬ 
ordinated  turn,  i.e.,  at  a  constant  turn  rate  with  a  con¬ 
stant  speed.  For  the  state  vector  defined  in  (25),  the 
coordinated  turn  model  is  given  by 
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(31) 


where  u  is  the  turn  rate  considered  to  be  a  known 
modeling  parameter  and  is  defined  as  for  (27). 

For  the  IMM  filter,  three  models  are  specified  by 
choosing  different  values  for  lo.  In  the  first  model, 
setting  cu  =  0  in  (31)  leads  to  the  nearly  constant  velocity 
or  non-maneuver  model  (27).  In  the  second  model, 
UJ  =  5.7  deg/s  represents  a  left  turn  maneuver  while  in 
the  third  model,  lu  =  —5.7  deg/s  represents  a  right  turn 
maneuver.  The  three  models  have  an  equal  initial  model 
probability  of  1/3  and  the  model  transition  probability 
matrix  is  taken  as 
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(32) 
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Sample  run  of  nonlinear  constrained  tracking  solution 


Fig.  9.  Sample  trajectories  for  unconstrained  versus  constrained  IMM. 


The  three  interacting  filters  inside  the  IMM  tracker 
are  identically  initialized  as  x,  ~  iV(xQ,PQ)  for  i  =  1,2,3- 
The  same  sensor  model  as  (26)  is  used  for  generat¬ 
ing  radar  measurements  in  this  simulation.  Hybrid  con¬ 
straints  are  applied  with  the  nonlinear  constraint  (14) 
for  position  estimates  and  the  linear  constraint  (30)  for 
velocity  estimates.  In  the  Monte  Carlo  simulation,  the 
truth  track  of  the  target  remains  the  same  but  the  initial 
estimate  is  drawn  from  the  distribution  for  each  run  as 
described  above.  So  is  the  measurement  noise  at  each 
sampling  instant  for  each  run. 

Fig.  9  shows  sample  trajectories  wherein  the  tar¬ 
get  starts  from  an  initial  position  at  (xQ,yo)  =  (100  m, 
—  100  m)  heading  due  north  along  a  straight  road  with 
y  =  10  m/s.  Atk=  10  s,  it  follows  the  curve  and  makes 
a  left  turn  at  a  rate  of  5.7  deg/s  for  16  s.  At  k  =  17  s, 
it  comes  to  another  straight  road  heading  due  west  with 
i  =  — 10  m/s  for  5  s. 

The  true  state  is  represented  by  a  series  of  dots  (•) 
plotted  on  the  solid  line  of  road  segment.  The  corre¬ 
sponding  measurements  are  a  series  of  circles  (o).  The 
unconstrained  IMM  filter  is  shown  as  a  series  of  con¬ 
nected  stars  (*)  whereas  the  constrained  IMM  filter  is 
shown  as  a  series  of  connected  crosses  (x).  When  the 
target  is  on  linear  road  segments,  the  linear  constrained 
solution  (5)  is  applied  to  the  combined  state  of  the  IMM 
filter  while  on  the  curved  road  segment,  a  hybrid  con¬ 
strained  solution  is  used  (nonlinear  for  position  and  lin¬ 
ear  for  velocity).  From  Fig.  9,  the  typical  behavior  of 
an  unconstrained  IMM  filter  can  be  seen.  It  converges 
rather  quickly  from  the  initialization  of  large  errors,  de¬ 
velops  an  overshoot  right  after  the  maneuver  but  cor¬ 
rects  itself  towards  the  true  trajectory,  and  converges 


again  after  the  maneuver  terminated.  However,  these 
unconstrained  IMM  estimates  (*)  are  off  road  while  the 
target  is  on  road. 

In  contrast,  the  constrained  IMM  estimates  (x)  are 
always  on  road  even  though  they  do  not  fall  exactly  on 
top  of  the  true  positions  (•).  As  a  result,  the  constrained 
position  errors  are  smaller  than  the  unconstrained  ones 
as  shown  in  Fig.  10,  which  are  obtained  by  a  Monte 
Carlo  simulation  with  100  runs.  In  particular,  the  ve¬ 
locity  errors  of  the  unconstrained  IMM  solution  grow 
during  the  maneuver  period  whereas  those  of  the  con¬ 
strained  solution  appear  to  be  uniform. 

The  RMS  errors  averaged  over  the  entire  trajectory 
are  summarized  in  Table  II.  The  values  in  Table  II 
are  bigger  than  those  in  Table  I  because  of  larger 
initialization  errors  and  longer  simulation  run.  It  shows 
an  improvement  of  approximately  3  folds  in  position 
and  in  velocity. 

5.  CONCLUSIONS 

In  this  paper,  we  presented  an  approach  to  incorpo¬ 
rating  road  information  into  target  tracking  via  track-to- 
road  fusion.  In  this  approach,  road  segments  were  mod¬ 
eled  with  analytic  functions  and  their  fusion  with  a  tar¬ 
get  track  was  cast  as  a  linearly  or  nonlinearly  state  con¬ 
strained  optimization  procedure.  With  the  Lagrangian 
multiplier,  a  closed-form  solution  was  found  for  linear 
constraints  and  an  iterative  solution  for  nonlinear  con¬ 
straints.  Geometric  interpretations  of  the  solutions  were 
provided  for  simple  cases.  Computer  simulation  results 
demonstrate  the  performance  of  the  algorithms. 

Future  work  includes  both  algorithms  development 
and  practical  applications.  It  is  of  interest  to  extend 
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Monte  Carlo  run  for  nonlinear  constrained  tracking  solution 


Fig.  10.  Position  and  velocity  error  RMS  versus  time  (100  Monte  Carlo  runs). 


the  iterative  method  presented  in  the  paper  for  second- 
order  nonlinear  state  constraints  to  other  types  of  non¬ 
linear  constraints  of  practical  significance  and  to  search 
for  more  efficient  root-finding  algorithms  to  solve  for 
the  Lagrangian  multiplier.  Similarly,  the  simple  fusion 
of  a  single  track  to  a  single  road  as  presented  in  this 
paper  is  being  extended  to  multiple  targets  moving 
along  closely-spaced  road  networks  with  intersections 
and  by-passes.  In  this  case,  the  fusion  (or  constrain¬ 
ing)  can  take  place  in  the  measurement  level  as  well 
as  in  the  track  level,  involving  road  constrained  data 
association  (RCDA).  Results  will  be  reported  in  future 
papers. 
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APPENDIX  A 

To  solve  the  constrained  optimization  problem  in 
(4),  we  form  the  cost  function  including  the  Lagrangian 
multiplier 

y(x,A)  =  (x-i)^W(x-i)-t2A^(Dx-d).  (Al) 

The  first  order  conditions  necessary  for  a  minimum 
are  given  by 

d  J 

—  =0^W(x-i)  +  D^A  =  0  (A2a) 

ox 


TABLE  II 

RMS  Estimation  Errors 


RMS  Estimation  Error 

Estimators 

Position  (m) 

Velocity  (m/s) 

Unconstrained  IMM 

9.0 

6.2 

Nonlinear  Constrained  IMM 

3.2 

1.7 

-d  =  0. 

(A2b) 

The  solution  for  the  optimal  Lagrangian  multiplier 
A  can  be  found  first  as 


A  =  (DW-‘D^)-HDi-d).  (A3) 

Bringing  this  solution  back  to  (Al)  leads  to  the 
constrained  solution  of  the  state  in  (5). 

Note  that  the  above  derivation  does  not  depend  on 
the  conditional  Gaussian  nature  of  the  unconstrained 
estimate  x.  It  was  shown  in  [24]  that  when  W  =  I, 
the  solution  in  (5)  is  the  same  as  what  is  obtained  by 
the  mean  square  method,  which  attempts  to  minimize 
the  conditional  mean  square  error  subject  to  the  state 
constraints,  that  is, 

minfilllx  — xll?  I  T}  such  that  Dx  =  d.  (A4) 

X 

Furthermore,  when  W  =  P^',  i.e.,  the  inverse  of  the 
unconstrained  state  estimation  error  covariance,  the  so¬ 
lution  in  (5)  reduces  to  the  result  given  by  the  maximum 
conditional  probability  method 

maxlnProbjx  I  F}  such  that  Dx  =  d.  (A5) 

X 
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{xi,  ^2}  ~  Basis  for  Original  Space 
^2}  ~  Basis  for  Translated  Space 
5  =  {x:  Dx  =  d}  ~  Constraint  Surface 
in  Original  Space 

L  =  {^:  D^=  0}  ~  Constraint  Surface 
in  Translated  Surface  (Subspace) 

T)  =  span{T)^  ~  Subspace  Orthogonal 
i^j  to  Constraint  Subspace 

D®  £  =  Translated  Space 
Xq  ~  Arbitrary  Point  on  Constraint 
Surface  in  Original  Space 
X  ~  Unconstrained  State  in  Original  Space 
Unconstrained  State  in  Translated  Space 
Xj  ~  Constrained  State  in  Translated  Space 
4))  ~  Projection  onto  V 
X*  ~  Constrained  State  in  Original  Space 

1  of  linear  constrained  solution. 


More  results  and  proofs  can  be  found  in  [24]. 


APPENDIX  B 


For  X  G  K",  the  constraint  surface  5  =  {x  :  Dx  =  d} 
with  the  number  of  linear  constraints  m  <n  is  not  a 
subspace  simply  because  for  d  7^  0,  the  null  vector  is 
not  inside  S.  To  construct  a  subspace,  first  find  an 
arbitrary  point  Xq  G  5  and  then  define  ^  =  x  —  Xg.  This 
is  equivalent  to  shifting  the  origin  of  the  coordinates  to 
Xg,  thus  performing  an  affine  transformation,  denoted 
by  T.  For  all  x  G  5,  the  corresponding  shifted  vector  | 
has  the  following  property; 

=  D(x-Xo)  =  Dx-Dxg  =  d-d  =  0.  (Bl) 

In  other  words,  the  constraint  surface  after  the  affine 
transformation  T  becomes  a  subspace,  denoted  by  £  = 
T5  =  {I :  =  0},  which  has  a  dimension  n  —  m.  The 

affine  transformation  is  illustrated  in  Fig.  Bl. 

We  are  now  to  express  C.  But  first,  the  row  vectors 
of  D  can  be  expressed  as: 

D^  =  [d,  d2---dj.  (B2) 

Since  D  is  of  full  rank  by  assumption,  the  row 
vectors  of  D  can  be  used  as  the  non-orthogonal  basis 
for  a  subspace  denoted  by  22  =  span{di,d2,. . .,d„j}.  In 
light  of  (Bl)  and  by  definition  of  £,  it  is  easy  to  see  that 
T>  is  an  orthogonal  complement  of  £,  that  is,  22  _L  £  and 
22  0  £  =  K"  where  ©  stands  for  direct  sum  between  two 
orthogonal  subspaces. 

For  6  G  22,  it  can  be  written  as; 


6  =  ^c,d,  =  [d,  d2---d„ 


/=1 


■  C,^A 


=  D^c.  (B3) 


Then  for  ^  G  we  have 

(6,1)  =  (D^c,|)  =  6^  =  =  0  (B4) 

where  (a,  b)  =  a^b  is  the  inner  product  defined  on  K" . 

By  the  principle  of  orthogonality,  an  arbitrary  vector 
I  can  be  decomposed  into  its  projections  onto  the 
orthogonal  complements  22  and  £,  denoted  by  and 
respectively,  as 

i  =  iD  +  ii-  (B5) 

Adding  Xg  to  both  sides  of  (B5),  we  can  express  the 
vectors  in  the  original  coordinates  as: 

x  =  ^+Xo  =  |o+Il+Xo  =  Io+x*.  (B6) 

The  projection  of  the  arbitrary  vector  on  the  con¬ 
straint  subspace  £  and  the  constraint  surface  S  can  be 
obtained,  respectively,  as: 


iL=i-iD  (B7a) 

x*=x-^o-  (B7b) 


To  obtain  express  it  as  a  linear  combination 
of  the  non-orthogonal  bases  of  with  the  coefficient 
vector  c  as: 


=  [di  d2---d„ 


i=  1 


.C„,J 


=  D^c.  (B8) 


Again,  by  the  principle  of  orthogonality,  the  projec¬ 
tion  error  vector  ^  is  orthogonal  to  22,  i.e.,  each 
and  every  basis  of  it: 


-  |o,d,)  =  (4  -  D^c,d,)  D^c)  =  0, 


i  =  (B9) 
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Stacking  these  orthogonality  conditions,  we  obtain 


(I  -  D^c)  =  0  or  D(^  -  D^c)  =  0. 


Ld 


rj 

m 


(BIO) 


Since  DD^  is  an  m  x  m  matrix  and  invertible,  the  coef¬ 
ficient  vector  can  be  obtained  as: 


c  =  (DD^)-'D|.  (Bll) 

Bringing  (Bll)  back  to  (B8)  gives  the  projection 
vector  as: 

lo  =  D^(DD^)-‘D|  =  (B12) 

where  P  =  D^(DD^)-iD  is  usually  referred  to  as  the 
projection  matrix  onto  T>  and  (I  —  P)  is  the  projection 
matrix  onto  L. 

Bringing  (B12)  back  to  (B7)  gives 

^^  =  ^-P^  =  (I-P)^  (B13a) 

X*  =x-P^  =x-P(x-Xo).  (B13b) 

Bringing  the  expression  for  P  into  (B13b)  gives 

X*  =  X  -  D^(DD^)-iD(x  -  Xo) 

=  X  -  D^(DD^)-i(Dx  -  Dxo) 

=  x-D^(DD^)^HDx-d)  (B14) 

where  Dxq  =  d  is  used  to  arrive  at  the  last  equation 
because  of  Xg  e  5. 

Clearly,  (B14)  is  exactly  the  same  as  (5)  when  W  = 
I.  This  offers  a  geometric  interpretation  that  the  linear 
constrained  estimation  is  the  orthogonal  projection  of 
the  unconstrained  estimate  onto  the  constrained  surface. 
It  provides  a  theoretical  justification  of  the  intuitive 
practice  of  finding  a  point  along  the  road  that  is  of  the 
shortest  distance. 


APPENDIX  C 

When  W  7^  I,  we  can  rewrite  the  weighted  square 
error  formulation  as 

X  =  argmin(x  —  x)^W(x  —  x) 

xeS 

=  argmin[W^'^^(x  — x)]^W''^^(x  — x)  =  argminz^z 

zeS 

(Cl) 

where  W  =  wC2wC2  is  a  symmetric  positive  definite 
weighting  matrix.  This  can  be  understood  as  if  we 
perform  an  equivalent  un-weighted  optimization  on  the 
transformed  state: 

z  =  wC2x.  (C2) 


The  constraint  can  be  written  as: 

Dx  =  DW^C2yv1/2x  =  Mz  =  d  (C3) 

where  M  =  DW^C2  definition.  The  constrained  sur¬ 
face  5'  =  {z  :  Mz  =  d}  is  used  in  the  last  equality  of  (Cl). 

Since  the  constrained  solution  in  (B14)  holds  for  z 
with  M  and  d,  we  have 

z*  =  z  -  M2’(MM2’)-1(Mz  -  d).  (C4) 

Putting  (C2)  and  (C3)  into  (C4)  gives 

WC2x*  =  WC2x- W^C2D2’(-p^-l/2^-l/2pr^-l 

x(DW^C2wi/2x_d).  (C5) 

Multiplying  both  sides  by  W^C2  gives  the  weighted 
constrained  solution  as: 

X*  =x-W-^D^  (DW^  ‘  D^")- 1  (Dx  -  d)  (C6) 

which  is  exactly  the  same  as  (5). 

It  is  interesting  to  note  that  the  use  of  W  =  P^'  has 
the  effect  of  pre-whitening  in  the  sense  that 

Ejzz^’}  =  p-C2E{xx2'}p-l/2  =  p-l/2pp-l/2  ^  j_ 

(C7) 

APPENDIX  D 

Construct  the  Lagrangian  with  the  Lagrangian  mul¬ 
tiplier  A  as 

/(x.  A)  =  (z  -  Hx)2’(z  -  Hx)  -t-  A/(x).  (Dl) 

Taking  the  partial  derivatives  of  7(x,A)  with  respect 
to  X  and  A,  respectively,  setting  them  to  zero  leads  to 
the  necessary  conditions 

-H^’z  -t  Am  -I-  (H^’H  -t  AM)x  =  0  (D2a) 

x^^Mx  +  m^^x  +  x^^m  +  mQ  =  0.  (D2b) 

Assume  that  the  inverse  matrix  of  +  AM  ex¬ 
ists.  Then,  x  can  be  solved  from  (D2a),  giving  the  con¬ 
strained  solution  in  terms  of  the  unknown  A  as 

X  =  (H^'H  -t  AM)- 1  (H^’z  -  Am)  (D3) 

which  reduces  to  the  unconstrained  least-squares  solu¬ 
tion  when  A  =  0. 

Assume  that  the  matrix  M  admits  the  factorization 
M  =  and  apply  the  Cholesky  factorization  to  W  = 
as 

W  =  G^G  (D4) 

where  G  is  an  upper  right  diagonal  matrix.  We  then 
perform  the  SVD  [17]  of  the  matrix  LG^^  as 

LG-*  =  USV^  (D5) 

where  U  and  V  are  orthonormal  matrices  and  S  is  a 
diagonal  matrix  with  its  diagonal  elements  denoted  by 
a-.  For  a  square  matrix,  this  becomes  the  eigenvalue 
decomposition. 
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Introduce  two  new  vectors 


e(A)  =  [. . . e,.(A), ...V  =  (H^z -  Am) 


t  =  ....f  =  V^(G^)-'m. 


(D6a) 

(D6b) 


With  these  factorizations  and  new  matrix  and  vec¬ 
tor  notations,  the  constrained  solution  in  (D3)  can  be 
simplified  into  (16a),  which  is  repeated  below  for  easy 
reference  as 

X  =  G^*V(I-i- AS^S)-*e(A).  (D7) 

The  first  and  second  order  terms  of  x  in  (D2b)  can 
be  expressed  in  A  as: 

x^Mx  =  e(A)^(I  +  AS^S)-^S^S(I  +  AS^S)-*e(A) 


(1-1-  Xcrf)^ 


(D8a) 


''x  =  t^(I  -t-  AS^S)-ie(A)  =  X]  Y 


g;(AX- 

.  -I-  Act? 


(D8b) 


x^m  =  e(A)^(I  -t  AS^S)-*t  =  j 


g;(AX- 

.  -I-  Act?  ’ 


(D8c) 


Bringing  these  terms  into  the  constrained  equation 
in  (D2b)  gives  rise  to  the  constraint  equation,  now  ex¬ 
pressed  in  terms  of  the  unknown  Lagrangian  multiplier 
A,  as 

q(\)  =  (z^H  -  Am^)(H^H  -I-  AM)-2(H^z  -  Am) 

-r  m^(H^H  +  AM)-‘(H^z  -  Am) 

-r  (z^H  -  Am^)(H^H  -r  AM)-'m  -r  m,, 

=  e(A)^(I  -r  AS^S)-‘S^S(I  -t-  AS^S)-‘e(A) 

+  t^(I  H-  AS^S)-‘e(A)  +  e(A)^(I  -i-  AS^S)-‘t  -r  ntg 


•  (1  +  Xaf)^ 


which  is  (16b)  given  in  Section  3. 
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